//------------------------------------------------------------------------------
// Corobot Service
//
// Professional Microsoft Robotics Developer Studio
//
// Updated for RDS 2008 (V2.0)
//
// Generated from: dssnewservice /Service:"Corobot" /Namespace:"ProMRDS.Simulation.Corobot" /year:"2007" /month:"07"
//
//------------------------------------------------------------------------------

using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Xml;
using corobot = ProMRDS.Simulation.Corobot;

#region Simulation namespaces
using Microsoft.Robotics.Simulation;
using Microsoft.Robotics.Simulation.Engine;
using engineproxy = Microsoft.Robotics.Simulation.Engine.Proxy;
using Microsoft.Robotics.Simulation.Physics;
using Microsoft.Robotics.PhysicalModel;
#endregion

using xna = Microsoft.Xna.Framework;
using xnagrfx = Microsoft.Xna.Framework.Graphics;


namespace ProMRDS.Simulation.Corobot
{

    /// <summary>
    /// Implementation class for Corobot
    /// </summary>
    [DisplayName("Corobot")]
    [Description("Simulates a Corobot Robot")]
    [Contract(Contract.Identifier)]
    public class CorobotService : DsspServiceBase
    {
        [Partner("Engine",
        Contract = engineproxy.Contract.Identifier,
        CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
        private engineproxy.SimulationEnginePort _engineStub =
            new engineproxy.SimulationEnginePort();


        /// <summary>
        /// _state
        /// </summary>
        private CorobotState _state = new CorobotState();
        /// <summary>
        /// _main Port
        /// </summary>
        [ServicePort("/corobot", AllowMultipleInstances = false)]
        private CorobotOperations _mainPort = new CorobotOperations();
        /// <summary>
        /// Default Service Constructor
        /// </summary>
        public CorobotService(DsspServiceCreationPort creationPort)
            :
                base(creationPort)
        {
        }
        /// <summary>
        /// Service Start
        /// </summary>
        protected override void Start()
        {
            base.Start();

            // create a very simple world and add a Corobot to it

            // MainCamera initial view
            CameraView view = new CameraView();
            view.EyePosition = new Vector3(-1.65f, 1.63f, -0.29f);
            view.LookAtPoint = new Vector3(0, 0, 0);
            SimulationEngine.GlobalInstancePort.Update(view);

            // Add a SkyDome.
            SkyDomeEntity sky = new SkyDomeEntity("skydome.dds", "sky_diff.dds");
            SimulationEngine.GlobalInstancePort.Insert(sky);

            // Add a directional light to simulate the sun.
            LightSourceEntity sun = new LightSourceEntity();
            sun.State.Name = "Sun";
            sun.Type = LightSourceEntityType.Directional;
            sun.Color = new Vector4(0.8f, 0.8f, 0.8f, 1);
            sun.Direction = new Vector3(0.5f, -.75f, 0.5f);
            SimulationEngine.GlobalInstancePort.Insert(sun);

            // create a large horizontal plane, at zero elevation.
            /*
            // Use some real terrain instead
            TerrainEntity ground = new TerrainEntity(
                             "terrain.bmp",
                             "terrain_tex.jpg",
                             new MaterialProperties("ground",
                                 0.2f, // restitution
                                 0.5f, // dynamic friction
                                 0.5f) // static friction
                             );

            SimulationEngine.GlobalInstancePort.Insert(ground);
            */
            HeightFieldEntity ground = new HeightFieldEntity(
                "Ground", // name
                "Gravel.dds", // texture image
                new MaterialProperties("ground",
                    0.2f, // restitution
                    0.5f, // dynamic friction
                    0.5f) // static friction
                );
            SimulationEngine.GlobalInstancePort.Insert(ground);
            
            Vector3 dimensions = new Vector3(2f, 2f, 2f); // meters

            // create simple movable entity, with a single shape
            SingleShapeEntity box = new SingleShapeEntity(
                new BoxShape(
                    new BoxShapeProperties(
                    100, // mass in kilograms.
                    new Pose(), // relative pose
                    dimensions)), // dimensions
                    new Vector3(0, 1f, 29f * 2.54f / 100f + 1f));

            // Name the entity. All entities must have unique names
            box.State.Name = "box";

            // Insert entity in simulation.  
            SimulationEngine.GlobalInstancePort.Insert(box);

            // create a Corobot
            SimulationEngine.GlobalInstancePort.Insert(new CorobotEntity("Corobot", new Vector3(0, 0, 0)));
        }

        /// <summary>
        /// Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetHandler(Get get)
        {
            get.ResponsePort.Post(_state);
            yield break;
        }
    }

    /// <summary>
    /// An entity which represents a Corobot.  This entity is created facing in the +Z direction meaning
    /// that its front has positive Z coordinates.  Front and Rear IR sensors are included along with a front-facing camera.
    /// </summary>
    [DataContract]
    [DataMemberConstructor]
    public class CorobotEntity : VisualEntity
    {
        private static float InchesToMeters(float inches)
        {
            return (float)(inches * 2.54 / 100.0);
        }

        static float mass = 3.63f; // kg
        static float chassisClearance = InchesToMeters(1.5f);
        static float wheelGap = InchesToMeters(3f / 8f);
        static float wheelWidth = InchesToMeters(2.2f);
        static float wheelDiameter = 0.10f; // 10 cm
        static float wheelMass = 0.1f; // kg
        static float platformClearance = InchesToMeters(5.75f);

        static Vector3 platformDimensions = new Vector3(
            InchesToMeters(11.0f),
            InchesToMeters(3.0f),
            InchesToMeters(8.5f));
        static Vector3 chassisDimensions = new Vector3(
            platformDimensions.X - 2 * wheelWidth - 2 * wheelGap,
            InchesToMeters(2.5f),
            platformDimensions.Z);
        static Vector3 wheelFRPosition = new Vector3(
            chassisDimensions.X / 2.0f + wheelGap + wheelWidth / 2.0f,
            wheelDiameter / 2.0f,
            -InchesToMeters(5.5f - 2f));
        static Vector3 wheelFLPosition = new Vector3(
            -wheelFRPosition.X,
            wheelFRPosition.Y,
            wheelFRPosition.Z);
        static Vector3 wheelRRPosition = new Vector3(
            wheelFRPosition.X,
            wheelFRPosition.Y,
            -wheelFRPosition.Z);
        static Vector3 wheelRLPosition = new Vector3(
            -wheelFRPosition.X,
            wheelFRPosition.Y,
            -wheelFRPosition.Z);

        // instance variables
        RotatingWheelEntity _wheelFR;
        RotatingWheelEntity _wheelFL;
        RotatingWheelEntity _wheelRR;
        RotatingWheelEntity _wheelRL;

        [Category("Wheels")]
        [DataMember]
        public RotatingWheelEntity FrontRightWheel
        {
            get { return _wheelFR; }
            set { _wheelFR = value; }
        }

        [Category("Wheels")]
        [DataMember]
        public RotatingWheelEntity FrontLeftWheel
        {
            get { return _wheelFL; }
            set { _wheelFL = value; }
        }

        [Category("Wheels")]
        [DataMember]
        public RotatingWheelEntity RearRightWheel
        {
            get { return _wheelRR; }
            set { _wheelRR = value; }
        }

        [Category("Wheels")]
        [DataMember]
        public RotatingWheelEntity RearLeftWheel
        {
            get { return _wheelRL; }
            set { _wheelRL = value; }
        }

        [Category("Wheels")]
        [DataMember]
        public int FrontRightEncoder
        {
            get { return (int)(_wheelFR.Rotations * 600f); }
            set { _wheelFR.Rotations = (float)value / 600f; }
        }

        [Category("Wheels")]
        [DataMember]
        public int FrontLeftEncoder
        {
            get { return (int)(_wheelFL.Rotations * 600f); }
            set { _wheelFL.Rotations = (float)value / 600f; }
        }

        bool _isEnabled;
        /// <summary>
        /// True if drive mechanism is enabled
        /// </summary>
        [DataMember]
        [Description("True if the drive mechanism is enabled.")]
        public bool IsEnabled
        {
            get { return _isEnabled; }
            set { _isEnabled = value; }
        }

        float _motorTorqueScaling;
        /// <summary>
        /// Scaling factor to apply to motor torque requests
        /// </summary>
        [DataMember]
        [Description("Scaling factor to apply to motor torque requests.")]
        public float MotorTorqueScaling
        {
            get { return _motorTorqueScaling; }
            set { _motorTorqueScaling = value; }
        }


        /// <summary>
        /// Default constructor used when this entity is deserialized
        /// </summary>
        public CorobotEntity()
        {
        }

        /// <summary>
        /// Initialization constructor used when this entity is built programmatically
        /// </summary>
        /// <param name="initialPos"></param>
        public CorobotEntity(string name, Vector3 initialPos)
        {
            base.State.Name = name;
            base.State.Pose.Position = initialPos;
            _motorTorqueScaling = 10.16f;

            // add child entities
            InsertEntityGlobal(
                new CorobotIREntity(name + "_rearIR", 
                new Pose(initialPos + new Vector3(0, chassisDimensions.Y / 2.0f + chassisClearance, chassisDimensions.Z / 2.0f))));
            InsertEntityGlobal(
                new CorobotIREntity(name + "_frontIR",
                new Pose(initialPos + new Vector3(0, chassisDimensions.Y / 2.0f + chassisClearance, -chassisDimensions.Z / 2.0f),
                    TypeConversion.FromXNA(xna.Quaternion.CreateFromAxisAngle(new xna.Vector3(0, 1, 0), (float)Math.PI)))));

            CameraEntity frontCam = new CameraEntity(320, 240, (float)(30.0 * Math.PI / 180.0));
            frontCam.CameraModel = CameraEntity.CameraModelType.AttachedChild;
            frontCam.State.Name = name + "_cam";
            frontCam.IsRealTimeCamera = true;    // update each frame
            frontCam.State.Pose.Position = new Vector3(
                0,
                chassisDimensions.Y / 2.0f + chassisClearance - 0.01f,
                -chassisDimensions.Z / 2.0f) + initialPos;
            InsertEntityGlobal(frontCam);
        }

        public override void Initialize(xnagrfx.GraphicsDevice device, PhysicsEngine physicsEngine)
        {
            try
            {
                // chassis
                BoxShapeProperties chassisDesc = new BoxShapeProperties(
                    "chassis",
                    mass / 2.0f,
                    new Pose(new Vector3(0, chassisClearance + chassisDimensions.Y / 2.0f, 0)),
                    chassisDimensions);

                chassisDesc.Material = new MaterialProperties("chassisMaterial", 0.0f, 0.5f, 0.5f);

                BoxShape chassis = new BoxShape(chassisDesc);
                chassis.State.Name = "ChassisShape";
                base.State.PhysicsPrimitives.Add(chassis);
                //base.State.Flags = EntitySimulationModifiers.Kinematic;

                // platform
                BoxShapeProperties platformDesc = new BoxShapeProperties(
                    "platform",
                    mass / 2.0f,
                    new Pose(new Vector3(0, platformClearance + platformDimensions.Y / 2.0f, 0)),
                    platformDimensions);
                platformDesc.Material = chassisDesc.Material;
                BoxShape platform = new BoxShape(platformDesc);
                platform.State.Name = "PlatformShape";
                base.State.PhysicsPrimitives.Add(platform);

                base.CreateAndInsertPhysicsEntity(physicsEngine);
                base.PhysicsEntity.SolverIterationCount = 128;

                // Wheels
                WheelShapeProperties wheelFRprop = new WheelShapeProperties("FrontRightWheel", wheelMass, wheelDiameter / 2.0f);
                WheelShapeProperties wheelFLprop = new WheelShapeProperties("FrontLeftWheel", wheelMass, wheelDiameter / 2.0f);
                WheelShapeProperties wheelRRprop = new WheelShapeProperties("RearRightWheel", wheelMass, wheelDiameter / 2.0f);
                WheelShapeProperties wheelRLprop = new WheelShapeProperties("RearLeftWheel", wheelMass, wheelDiameter / 2.0f);

                wheelFRprop.Flags |= WheelShapeBehavior.OverrideAxleSpeed;
                wheelFLprop.Flags |= WheelShapeBehavior.OverrideAxleSpeed;
                wheelRRprop.Flags |= WheelShapeBehavior.OverrideAxleSpeed;
                wheelRLprop.Flags |= WheelShapeBehavior.OverrideAxleSpeed;

                wheelFRprop.InnerRadius = 0.7f * wheelDiameter / 2.0f;
                wheelFLprop.InnerRadius = 0.7f * wheelDiameter / 2.0f;
                wheelRRprop.InnerRadius = 0.7f * wheelDiameter / 2.0f;
                wheelRLprop.InnerRadius = 0.7f * wheelDiameter / 2.0f;

                wheelFRprop.LocalPose = new Pose(wheelFRPosition);
                wheelFLprop.LocalPose = new Pose(wheelFLPosition);
                wheelRRprop.LocalPose = new Pose(wheelRRPosition);
                wheelRLprop.LocalPose = new Pose(wheelRLPosition);

                TireForceFunctionDescription LongitudalFunction = new TireForceFunctionDescription();
                LongitudalFunction.ExtremumSlip = 1.0f;
                LongitudalFunction.ExtremumValue = 0.02f;
                LongitudalFunction.AsymptoteSlip = 2.0f;
                LongitudalFunction.AsymptoteValue = 0.01f;
                LongitudalFunction.StiffnessFactor = 10000000.0f;

                TireForceFunctionDescription LateralFunction = new TireForceFunctionDescription();
                LateralFunction.ExtremumSlip = 1.0f;
                LateralFunction.ExtremumValue = 0.02f;
                LateralFunction.AsymptoteSlip = 2.0f;
                LateralFunction.AsymptoteValue = 0.01f;
                LateralFunction.StiffnessFactor = 1000000.0f;

                wheelFRprop.TireLongitudalForceFunction = LongitudalFunction;
                wheelFLprop.TireLongitudalForceFunction = LongitudalFunction;
                wheelRRprop.TireLongitudalForceFunction = LongitudalFunction;
                wheelRLprop.TireLongitudalForceFunction = LongitudalFunction;

                wheelFRprop.TireLateralForceFunction = LateralFunction;
                wheelFLprop.TireLateralForceFunction = LateralFunction;
                wheelRRprop.TireLateralForceFunction = LateralFunction;
                wheelRLprop.TireLateralForceFunction = LateralFunction;

                _wheelFR = new RotatingWheelEntity(wheelFRprop);
                _wheelFR.State.Name = base.State.Name + " FrontRightWheel";
                _wheelFR.Parent = this;
                _wheelFR.State.Assets.Mesh = "CorobotWheel.obj";
                _wheelFR.MeshRotation = new Vector3(0, 180, 0);   // flip the wheel mesh
                _wheelFR.Initialize(device, physicsEngine);

                _wheelFL = new RotatingWheelEntity(wheelFLprop);
                _wheelFL.State.Name = base.State.Name + " FrontLeftWheel";
                _wheelFL.Parent = this;
                _wheelFL.State.Assets.Mesh = "CorobotWheel.obj";
                _wheelFL.Initialize(device, physicsEngine);

                _wheelRR = new RotatingWheelEntity(wheelRRprop);
                _wheelRR.State.Name = base.State.Name + " RearRightWheel";
                _wheelRR.Parent = this;
                _wheelRR.State.Assets.Mesh = "CorobotWheel.obj";
                _wheelRR.MeshRotation = new Vector3(0, 180, 0);   // flip the wheel mesh
                _wheelRR.Initialize(device, physicsEngine);

                _wheelRL = new RotatingWheelEntity(wheelRLprop);
                _wheelRL.State.Name = base.State.Name + " RearLeftWheel";
                _wheelRL.Parent = this;
                _wheelRL.State.Assets.Mesh = "CorobotWheel.obj";
                _wheelRL.Initialize(device, physicsEngine);

                State.Assets.Mesh = "Corobot.obj";

                base.Initialize(device, physicsEngine);
                
                //SetMotorTorque(1, 1); // used to test the top speed of the entity
            }
            catch (Exception ex)
            {
                // clean up
                if (PhysicsEntity != null)
                    PhysicsEngine.DeleteEntity(PhysicsEntity);

                HasBeenInitialized = false;
                InitError = ex.ToString();
            }
        }

        const float SPEED_DELTA = 0.5f;
        Pose startPose;
        double totalTime = -1;
        public override void Update(FrameUpdate update)
        {
            // used to test the top speed of the entity
            //if (totalTime < 0)
            //{
            //    startPose = State.Pose;
            //    totalTime = update.ElapsedTime;
            //}
            //else
            //{
            //    double distance = Vector3.Length(State.Pose.Position - startPose.Position);
            //    double speed = distance / totalTime;
            //    totalTime += update.ElapsedTime;
            //    Console.WriteLine(speed.ToString());
            //}

            // update state for us and all the shapes that make up the rigid body
            PhysicsEntity.UpdateState(true);

            if (_distanceToTravel > 0)
            {
                // DriveDistance update
                double currentDistance = Vector3.Length(State.Pose.Position - _startPoseForDriveDistance.Position);
                if (currentDistance >= _distanceToTravel)
                {
                    _wheelFR.Wheel.AxleSpeed = 0;
                    _wheelFL.Wheel.AxleSpeed = 0;
                    _wheelRR.Wheel.AxleSpeed = 0;
                    _wheelRL.Wheel.AxleSpeed = 0;
                    _leftTargetVelocity = 0;
                    _rightTargetVelocity = 0;
                    _distanceToTravel = 0;
                    // now that we're finished, post a response
                    if (_driveDistancePort != null)
                    {
                        SuccessFailurePort tmp = _driveDistancePort;
                        _driveDistancePort = null;
                        tmp.Post(new SuccessResult());
                    }
                }
                else if ((DateTime.Now - _startTime).TotalSeconds > _timeoutSeconds)
                {
                    if (_driveDistancePort != null)
                    {
                        SuccessFailurePort tmp = _driveDistancePort;
                        _driveDistancePort = null;
                        tmp.Post(new Exception("Request timed out."));
                    }
                }
                else
                {
                    // need to drive further, check to see if we should slow down
                    if (progressPoints.Count >= averageKernel)
                    {
                        double distanceRemaining = _distanceToTravel - currentDistance;
                        double framesToCompletion = distanceRemaining * averageKernel / (currentDistance - progressPoints.Dequeue());
                        if (framesToCompletion < decelerateThreshold)
                        {
                            if (Math.Abs(_leftTargetVelocity) > 0.1)
                            {
                                _leftTargetVelocity *= 0.5f;
                                _rightTargetVelocity *= 0.5f;
                            }
                            progressPoints.Clear();
                        }
                    }
                    progressPoints.Enqueue(currentDistance);
                }
            }
            else if (_targetRotation != double.MaxValue)
            {
                // RotateDegrees update
                float currentHeading = CurrentHeading;
                double angleDelta = currentHeading - _previousHeading;
                while (angleDelta > Math.PI)
                    angleDelta -= twoPI;
                while (angleDelta <= -Math.PI)
                    angleDelta += twoPI;
                _currentRotation += angleDelta;
                _previousHeading = currentHeading;  // for next frame

                float angleError;
                if (_targetRotation < 0)
                    angleError = (float)(_currentRotation - _targetRotation);
                else
                    angleError = (float)(_targetRotation - _currentRotation);

                if (angleError < acceptableRotationError)
                {
                    // current heading is within acceptableError or has overshot
                    // end the rotation
                    _targetRotation = double.MaxValue;
                    _wheelFR.Wheel.AxleSpeed = 0;
                    _wheelFL.Wheel.AxleSpeed = 0;
                    _wheelRR.Wheel.AxleSpeed = 0;
                    _wheelRL.Wheel.AxleSpeed = 0;
                    _leftTargetVelocity = 0;
                    _rightTargetVelocity = 0;
                    // now that we're finished, post a response
                    if (_rotateDegreesPort != null)
                    {
                        SuccessFailurePort tmp = _rotateDegreesPort;
                        _rotateDegreesPort = null;
                        tmp.Post(new SuccessResult());
                    }
                }
                else if ((DateTime.Now - _startTime).TotalSeconds > _timeoutSeconds)
                {
                    if (_rotateDegreesPort != null)
                    {
                        SuccessFailurePort tmp = _rotateDegreesPort;
                        _rotateDegreesPort = null;
                        tmp.Post(new Exception("Request timed out."));
                    }
                }
                else
                {
                    if (angleDelta != 0)
                    {
                        // need to turn more, check to see if we should slow down
                        if (progressPoints.Count >= averageKernel)
                        {
                            double framesToCompletion = Math.Abs(angleError * averageKernel / (_currentRotation - progressPoints.Dequeue()));
                            if (framesToCompletion < decelerateThreshold)
                            {
                                if(Math.Abs(_leftTargetVelocity) > 0.1)
                                    _leftTargetVelocity *= 0.5f;
                                
                                if (Math.Abs(_rightTargetVelocity) > 0.1)
                                    _rightTargetVelocity *= 0.5f;
                                progressPoints.Clear();
                            }
                        }
                        progressPoints.Enqueue(_currentRotation);
                    }
                }
            }

            float left = _wheelFL.Wheel.AxleSpeed + _leftTargetVelocity;
            float right = _wheelFR.Wheel.AxleSpeed + _rightTargetVelocity;

            if (Math.Abs(left) > 0.1)
            {
                if (left > 0)
                    _wheelFL.Wheel.AxleSpeed -= SPEED_DELTA;
                else
                    _wheelFL.Wheel.AxleSpeed += SPEED_DELTA;
            }

            if (Math.Abs(right) > 0.1)
            {
                if (right > 0)
                    _wheelFR.Wheel.AxleSpeed -= SPEED_DELTA;
                else
                    _wheelFR.Wheel.AxleSpeed += SPEED_DELTA;
            }

            // match the rear wheels with the front wheels
            _wheelRL.Wheel.AxleSpeed = _wheelFL.Wheel.AxleSpeed;
            _wheelRR.Wheel.AxleSpeed = _wheelFR.Wheel.AxleSpeed;

            // update entities in fields
            _wheelFL.Update(update);
            _wheelFR.Update(update);
            _wheelRL.Update(update);
            _wheelRR.Update(update);

            // sim engine will update children
            base.Update(update);
        }

        public override void Render(RenderMode renderMode, MatrixTransforms transforms, CameraEntity currentCamera)
        {
            base.Render(renderMode, transforms, currentCamera);
            _wheelFL.Render(renderMode, transforms, currentCamera);
            _wheelFR.Render(renderMode, transforms, currentCamera);
            _wheelRL.Render(renderMode, transforms, currentCamera);
            _wheelRR.Render(renderMode, transforms, currentCamera);
        }

        #region Motor Base Control

        Pose _startPoseForDriveDistance;
        double _distanceToTravel;
        SuccessFailurePort _driveDistancePort = null;

        /// <summary>
        /// Applies constant power to both wheels, driving the motor base for a fixed distance, in the current direction
        /// </summary>
        /// <param name="distance">Distance to travel, in meters</param>
        /// <param name="power">Normalized power (torque) value for both wheels</param>
        /// <param name="responsePort">A port to report the result of the request, success or exception</param>
        public void DriveDistance(float distance, float power, SuccessFailurePort responsePort)
        {
            // reset any drivedistance or rotate degrees commands that haven't completed
            ResetRotationAndDistance();

            // keep track of the response port for when we complete the request
            _driveDistancePort = responsePort;

            // handle negative distances
            if (distance < 0)
            {
                distance = -distance;
                power = -power;
            }
            _startPoseForDriveDistance = State.Pose;
            _distanceToTravel = distance;
            SetAxleVelocity(power * _motorTorqueScaling, power * _motorTorqueScaling);
            _startTime = DateTime.Now;
        }

        // DriveDistance and RotateDegrees variables
        Queue<double> progressPoints = new Queue<double>();
        const int averageKernel = 6;
        const int decelerateThreshold = 6;
        const float twoPI = (float)(2 * Math.PI);
        DateTime _startTime;

        // RotateDegrees variables
        double _targetRotation = double.MaxValue;
        double _currentRotation = 0;
        double _previousHeading = 0;
        const float acceptableRotationError = 0.005f;
        SuccessFailurePort _rotateDegreesPort = null;

        float _timeoutSeconds = 15.0f;

        /// <summary>
        /// The timeout for DriveDistance and RotateDegrees commands in seconds.
        /// </summary>
        [DataMember]
        public float TimeoutSeconds
        {
            get { return _timeoutSeconds; }
            set { _timeoutSeconds = value; }
        }

        /// <summary>
        /// Applies constant power to each wheel (but of inverse polarity), rotating the motor base 
        /// through the given rotation.
        /// </summary>
        /// <param name="degrees">Rotation around Y axis, in degrees.</param>
        /// <param name="power">Normalized power (torque) value for both wheels</param>
        /// <param name="responsePort">A port to report the result of the request, success or exception</param>
        public void RotateDegrees(float degrees, float power, SuccessFailurePort responsePort)
        {
            // reset any drivedistance or rotate degrees commands that haven't completed
            ResetRotationAndDistance();

            // keep track of the response port for when we complete the request
            _rotateDegreesPort = responsePort;

            _targetRotation = xna.MathHelper.ToRadians(degrees);
            _currentRotation = 0;
            _previousHeading = CurrentHeading;

            if (degrees < 0)
                SetAxleVelocity(power * _motorTorqueScaling, -power * _motorTorqueScaling);
            else
                SetAxleVelocity(-power * _motorTorqueScaling, power * _motorTorqueScaling);

            _startTime = DateTime.Now;
        }

        /// <summary>
        /// Current heading, in radians, of robot base
        /// </summary>
        public float CurrentHeading
        {
            get
            {
                // return the axis angle of the quaternion
                xna.Vector3 euler = UIMath.QuaternionToEuler(State.Pose.Orientation);
                return xna.MathHelper.ToRadians(euler.Y); // heading is the rotation about the Y axis.
            }
        }

        /// <summary>
        /// When a direct update to motor torque or wheel velocity occurs
        /// we abandon any current DriveDistance or RotateDegrees commands
        /// </summary>
        /// <summary>
        /// When a direct update to motor torque or wheel velocity occurs
        /// we abandon any current DriveDistance or RotateDegrees commands
        /// </summary>
        void ResetRotationAndDistance()
        {
            progressPoints.Clear();
            _distanceToTravel = 0;
            _targetRotation = double.MaxValue;
            if (_driveDistancePort != null)
            {
                _driveDistancePort.Post(new Exception("Request superceded prior to completion."));
                _driveDistancePort = null;
            }
            if (_rotateDegreesPort != null)
            {
                _rotateDegreesPort.Post(new Exception("Request superceded prior to completion."));
                _rotateDegreesPort = null;
            }
        }


        /// <summary>
        /// Sets motor torque on the active wheels
        /// </summary>
        /// <param name="leftWheel"></param>
        /// <param name="rightWheel"></param>
        public void SetMotorTorque(float leftWheel, float rightWheel)
        {
            ResetRotationAndDistance();
            SetAxleVelocity(leftWheel * _motorTorqueScaling, rightWheel * _motorTorqueScaling);
        }

        float _leftTargetVelocity;
        float _rightTargetVelocity;

        /// <summary>
        /// Sets angular velocity (radians/sec) on both wheels
        /// </summary>
        /// <param name="value"></param>
        public void SetVelocity(float value)
        {
            ResetRotationAndDistance();
            SetVelocity(value, value);
        }

        /// <summary>
        /// Sets angular velocity on the wheels
        /// </summary>
        /// <param name="left"></param>
        /// <param name="right"></param>
        public void SetVelocity(float left, float right)
        {
            ResetRotationAndDistance();
            if (_wheelFR == null || _wheelFL == null)
                return;

            left = ValidateWheelVelocity(left);
            right = ValidateWheelVelocity(right);

            // v is in m/sec - convert to an axle speed
            //  2Pi(V/2PiR) = V/R
            SetAxleVelocity(left / _wheelFR.Wheel.State.Radius,
                right / _wheelFL.Wheel.State.Radius);
        }

        private void SetAxleVelocity(float left, float right)
        {
            _leftTargetVelocity = left;
            _rightTargetVelocity = right;
        }

        const float MAX_VELOCITY = 20.0f;
        const float MIN_VELOCITY = -MAX_VELOCITY;

        float ValidateWheelVelocity(float value)
        {
            if (value > MAX_VELOCITY)
                return MAX_VELOCITY;
            if (value < MIN_VELOCITY)
                return MIN_VELOCITY;

            return value;
        }
        #endregion
    }

    /// <summary>
    /// An IR distance sensor.
    /// </summary>
    public class CorobotIREntity : VisualEntity
    {
        [DataMember]
        public float DispersionConeAngle = 8f; // in degrees
        [DataMember]
        public float Samples = 3f;  // the number of rays in each direction
        [DataMember]
        public float MaximumRange = (30f * 2.54f / 100.0f); // 30 inches converted to meters

        float _elapsedSinceLastScan;
        Port<RaycastResult> _raycastResultsPort;
        RaycastResult _lastResults;
        Port<RaycastResult> _raycastResults = new Port<RaycastResult>();
        RaycastProperties _raycastProperties;
        CachedEffectParameter _timeAttenuationHandle;
        float _appTime;
        Shape _particlePlane;

        /// <summary>
        /// Raycast configuration
        /// </summary>
        public RaycastProperties RaycastProperties
        {
            get { return _raycastProperties; }
            set { _raycastProperties = value; }
        }

        float _distance;
        [DataMember]
        public float Distance
        {
            get { return _distance; }
            set { _distance = value; }
        }

        /// <summary>
        /// Default constructor used when this entity is deserialized
        /// </summary>
        public CorobotIREntity()
        {
        }

        /// <summary>
        /// Initialization constructor used when this entity is built programmatically
        /// </summary>
        /// <param name="initialPos"></param>
        public CorobotIREntity(string name, Pose initialPose)
        {
            base.State.Name = name;
            base.State.Pose = initialPose;

            // used for rendering impact points
            base.State.Assets.Effect = "LaserRangeFinder.fx";
        }

        public override void Initialize(xnagrfx.GraphicsDevice device, PhysicsEngine physicsEngine)
        {
            try
            {
                if (Parent == null)
                    throw new Exception("This entity must be a child of another entity.");

                // make sure that we take at least 2 samples in each direction
                if (Samples < 2f)
                    Samples = 2f;

                _raycastProperties = new RaycastProperties();
                _raycastProperties.StartAngle = -DispersionConeAngle / 2.0f;
                _raycastProperties.EndAngle = DispersionConeAngle / 2.0f;
                _raycastProperties.AngleIncrement = DispersionConeAngle / (Samples - 1f);
                _raycastProperties.Range = MaximumRange;
                _raycastProperties.OriginPose = new Pose();

                // set flag so rendering engine renders us last
                Flags |= VisualEntityProperties.UsesAlphaBlending;

                base.Initialize(device, physicsEngine);

                // set up for rendering impact points
                HeightFieldShapeProperties hf = new HeightFieldShapeProperties("height field", 2, 0.02f, 2, 0.02f, 0, 0, 1, 1);
                hf.HeightSamples = new HeightFieldSample[hf.RowCount * hf.ColumnCount];
                for (int i = 0; i < hf.HeightSamples.Length; i++)
                    hf.HeightSamples[i] = new HeightFieldSample();

                _particlePlane = new Shape(hf);
                _particlePlane.State.Name = "laser impact plane";

                // The mesh is used to render the ray impact points rather than the sensor geometry.
                int index = Meshes.Count;
                Meshes.Add(SimulationEngine.ResourceCache.CreateMesh(device, _particlePlane.State));
                Meshes[0].Textures[0] = SimulationEngine.ResourceCache.CreateTextureFromFile(device, "particle.bmp");

                // we have a custom effect, with an additional global parameter. Get handle to it here
                if (Effect != null)
                    _timeAttenuationHandle = Effect.GetParameter("timeAttenuation");

            }
            catch (Exception ex)
            {
                // clean up
                if (PhysicsEntity != null)
                    PhysicsEngine.DeleteEntity(PhysicsEntity);

                HasBeenInitialized = false;
                InitError = ex.ToString();
            }
        }

        const float SCAN_INTERVAL = 0.250f;

        public override void Update(FrameUpdate update)
        {
            base.Update(update);
            _elapsedSinceLastScan += (float)update.ElapsedTime;
            _appTime = (float)update.ApplicationTime;

            // only retrieve raycast results every SCAN_INTERVAL.
            // For entities that are compute intenisve, you should consider giving them
            // their own task queue so they dont flood a shared queue
            if ((_elapsedSinceLastScan > SCAN_INTERVAL) && (_raycastProperties != null))
            {
                _elapsedSinceLastScan = 0;

                _raycastProperties.OriginPose.Orientation = TypeConversion.FromXNA(
                    TypeConversion.ToXNA(Parent.State.Pose.Orientation) *
                    TypeConversion.ToXNA(State.Pose.Orientation));

                _raycastProperties.OriginPose.Position = TypeConversion.FromXNA(
                    xna.Vector3.Transform(TypeConversion.ToXNA(State.Pose.Position), Parent.World));

                // cast rays on a horizontal plane and again on a vertical plane
                _raycastResultsPort = PhysicsEngine.Raycast2D(_raycastProperties);
                _raycastResultsPort.Test(out _lastResults);
                if (_lastResults != null)
                {
                    RaycastResult verticalResults;

                    // rotate the plane by 90 degrees
                    _raycastProperties.OriginPose.Orientation =
                        TypeConversion.FromXNA(TypeConversion.ToXNA(_raycastProperties.OriginPose.Orientation) *
                        xna.Quaternion.CreateFromAxisAngle(new xna.Vector3(0, 0, 1), (float)Math.PI / 2f));

                    _raycastResultsPort = PhysicsEngine.Raycast2D(_raycastProperties);
                    _raycastResultsPort.Test(out verticalResults);

                    // combine the results of the second raycast with the first
                    if (verticalResults != null)
                    {
                        foreach (RaycastImpactPoint impact in verticalResults.ImpactPoints)
                            _lastResults.ImpactPoints.Add(impact);
                    }

                    // find the shortest distance to an impact point
                    float minRange = MaximumRange * MaximumRange;
                    xna.Vector4 origin = new xna.Vector4(TypeConversion.ToXNA(_raycastProperties.OriginPose.Position), 1);
                    foreach (RaycastImpactPoint impact in _lastResults.ImpactPoints)
                    {
                        xna.Vector3 impactVector = new xna.Vector3(
                            impact.Position.X - origin.X,
                            impact.Position.Y - origin.Y,
                            impact.Position.Z - origin.Z);

                        float impactDistanceSquared = impactVector.LengthSquared();
                        if (impactDistanceSquared < minRange)
                            minRange = impactDistanceSquared;
                    }
                    _distance = (float)Math.Sqrt(minRange);
                }
            }
        }
        /// <summary>
        /// Frame render
        /// </summary>
        public override void Render(RenderMode renderMode, MatrixTransforms transforms, CameraEntity currentCamera)
        {
            if ((int)(Flags & VisualEntityProperties.DisableRendering) > 0)
                return;

            if (_lastResults != null)
                RenderResults(renderMode, transforms, currentCamera);
        }

        void RenderResults(RenderMode renderMode, MatrixTransforms transforms, CameraEntity currentCamera)
        {
            _timeAttenuationHandle.SetValue(new xna.Vector4(100 * (float)Math.Cos(_appTime * (1.0f / SCAN_INTERVAL)), 0, 0, 1));

            // render impact points as a quad
            xna.Matrix inverseViewRotation = currentCamera.ViewMatrix;
            inverseViewRotation.M41 = inverseViewRotation.M42 = inverseViewRotation.M43 = 0;
            xna.Matrix.Invert(ref inverseViewRotation, out inverseViewRotation);
            xna.Matrix localTransform = xna.Matrix.CreateFromAxisAngle(new xna.Vector3(1, 0, 0), (float)-Math.PI / 2) * inverseViewRotation;

            SimulationEngine.GlobalInstance.Device.RenderState.DepthBufferWriteEnable = false;
            for (int i = 0; i < _lastResults.ImpactPoints.Count; i++)
            {
                xna.Vector3 pos = new xna.Vector3(_lastResults.ImpactPoints[i].Position.X,
                    _lastResults.ImpactPoints[i].Position.Y,
                    _lastResults.ImpactPoints[i].Position.Z);

                xna.Vector3 resultDir = pos - Parent.Position;
                resultDir.Normalize();

                localTransform.Translation = pos - .02f * resultDir;
                transforms.World = localTransform;

                base.Render(renderMode, transforms, Meshes[0]);
            }
            SimulationEngine.GlobalInstance.Device.RenderState.DepthBufferWriteEnable = true;
        }
    }

    [DataContract]
    public class RotatingWheelEntity : WheelEntity
    {
        const float rotationScale = (float)(-1.0 / (2.0 * Math.PI));
        public float Rotations = 0;

        public RotatingWheelEntity()
        {
        }

        public RotatingWheelEntity(WheelShapeProperties wheelShape)
            : base(wheelShape)
        {
        }

        public override void Initialize(Microsoft.Xna.Framework.Graphics.GraphicsDevice device, PhysicsEngine physicsEngine)
        {
            base.Initialize(device, physicsEngine);
        }

        public override void Update(FrameUpdate update)
        {
            base.Update(update);

            // set the wheel to the current position
            Wheel.State.LocalPose.Orientation = TypeConversion.FromXNA(
                xna.Quaternion.CreateFromAxisAngle(new xna.Vector3(-1, 0, 0), (float)(Rotations * 2 * Math.PI)));

            // update the rotations for the next frame
            Rotations += (float)(Wheel.AxleSpeed * update.ElapsedTime * rotationScale);
        }
    }
}
